#include "Matrix/LB_Matrix.h"
#include "Mesh/Cell_Traits.h"
#include "Mesh/_Func_.h"
#include "tool/libcell_tools_kd_node.h"
#include "tools/intrbtree.h"
#include "tools_node.h"
#include "tools_vector.h"
#include <cstdlib>
#include <cstring>
#include <stdio.h>
#include<Algorithm/mesh_smoothing.h>
#include<Mesh_IO/Mesh_IO.h>
#include<libcell_macros.h>
#include<tool/libcell_kd_node_iterator.h>
#include<Algorithm/threeD_elementary_geometry_tool_library.h>
#include<Algorithm/points_cloud_algorithm.h>
#define quote lib_cell_quote
#define _ReadOff_   LibCell_ReadOff_
#define _ReadCell_  LibCell_ReadCell_
#define _ReadPly_ LibCell_ReadPly_

#define _WriteCell_ LibCell_WriteCell_
#define _WriteOff_ LibCell_WriteOff_

#include<time.h>
#include<thread>

void test()
{
    Mesh m;
    Mesh_init(&m);

    _ReadOff_(&m,"face.off",3);
    Node* node=NULL;
    for(auto vit=m.v_begin(&m);vit!=m.v_end(&m);vit++)
    {
        node=node_overlying(node,quote(vit));

    }

    clock_t t=clock();
    // mesh_smoothing_taubin(&m,0.5,-0.25,10);

    // std::thread t1(mesh_smoothing_taubin,&m,0.5,-0.25,5);
    mesh_smoothing_taubin_noden(node,0.5,-0.25,40);

    // t1.join();

    printf("time si %.5f \n",(double)(clock()-t)/CLOCKS_PER_SEC );
    // for(Node* nit=node;nit!=NULL;nit=(Node*)(nit->Next))
    // {
    //     template_v* v=(template_v*)(nit->value);
    //     printf("%lf %lf %lf\n",v->point[0],v->point[1],v->point[2]);

    // }
}

// 0.282918 0.720018 0.336696
// 0.264556 0.721649 0.327768
// 0.242640 0.720878 0.312205
// 0.228380 0.722093 0.304740
// 0.213652 0.723990 0.298598
// 0.198722 0.726157 0.293475
// 0.184082 0.728152 0.288145
// 0.170308 0.730097 0.283484
// 0.157355 0.732312 0.280706
// 0.144786 0.735451 0.279464
// 0.132419 0.738047 0.277123
// 0.119522 0.741310 0.275770
// 0.106350 0.745858 0.275997
// 0.092923 0.749821 0.274915
// 0.079127 0.753893 0.273301
// 0.066275 0.757315 0.271430
// 0.054158 0.759569 0.269492
// 0.043003 0.762718 0.270392
// 0.034071 0.767252 0.275252
// 0.027017 0.772682 0.283208
// 0.005763 0.782193 0.296394
// -0.017357 0.786453 0.296370
// -0.042158 0.789232 0.292321
// -0.055294 0.790073 0.289854
// -0.070775 0.791054 0.288750
// -0.084504 0.794616 0.292553
// -0.102536 0.798388 0.296096
// -0.121555 0.801516 0.299979
// -0.133662 0.805338 0.305256
// -0.155135 0.811538 0.316068
// -0.185046 0.820012 0.331485
// -0.214993 0.823102 0.336223
//
static double get_nearest_point_from_v_cells(template_v*v,double*p)
{
    double *ps[3]={0};
    double lambda[3]={0};
    double re=-1;
    for(int i=0;i<v->cellsn.size;i++)
    {
        template_c*c= (template_c*)(*lb_vector1_at_voidptr(&(v->cellsn),i));
        ps[0]=c->vertices[0]->point;
        ps[1]=c->vertices[1]->point;
        ps[2]=c->vertices[2]->point;

        compute_neareat_point2simplex(ps,
            3,3,p,lambda );
        double tempp[3]={0};
        for(int j=0;j<3;j++)
        {
            tempp[j]=lambda[0]*ps[0][j]+lambda[1]*ps[1][j]+lambda[2]*ps[2][j];
        }
        double dis=distance_of_two_points(tempp,p,3);
        if(re<0||dis<re)
        {
            re=dis;
        }
    }

    return re;
}
// piont :10.251193 -2.586275 0.128862
// 49508
static int my_filter_vertex(template_v*v,KD_Node*kdtree,double dis)
{
    KD_Node*kdnode1=kd_tree_find_nearest(v,kdtree);
    // KD_Node*kdnode=kd_tree_find_nearest(vs[i],kdtree);
    template_v* v1=kdnode1->value;
    double dis1=distance_of_two_points(v1->point,
        v->point , 3);
    if(dis1<dis)
    {
        return 1;
    }
    return 0;
}

// kdtree2是固位网
// kdtree是大连接体
// kdtree3是终止线
// kdtreeb是大连接体边界
//
void mark_vertices(template_v**vs,int*marks,int len,
    KD_Node* kdtree,
    KD_Node*kdtree2,KD_Node*kdtree3,
    KD_Node* kdtreeb)
{
    for(int i=0;i<len;i++)
    {
        KD_Node*kdnode=kd_tree_find_nearest(vs[i],kdtree);
        template_v* v=kdnode->value;
        double dis=distance_of_two_points(v->point,
            vs[i]->point , 3);
        double dis1=0;
        if(dis<0.28)
        {
            marks[i]=0;
        }
        else
        {
            if(dis<2.0)
            {
                dis1= get_nearest_point_from_v_cells(v,vs[i]->point);
                if(dis1<0.28){
                    // printf("dis:%lf\n",dis);
                    marks[i]=0;}
                else {
                    marks[i]=1;
                }
            }
        }
        if(marks[i]==1)
        {
            if(my_filter_vertex(vs[i],kdtreeb,1.2)==1)
            {
                marks[i]=0;
                continue;
            }
        }
        if(marks[i]==1)
        {
            // 固位网0.8
            if(my_filter_vertex(vs[i],kdtree2,0.8)==1)
            {
                marks[i]=0;
                continue;
            }
            // if(marks[i]==1)
            // {
            //     if(my_filter_vertex(vs[i],kdtree3,0.4)==1)
            //     {
            //         marks[i]=0;
            //         continue;
            //     }
            // }
            // 终止线0.6
        }
        // if(vs[i]->id==49508||vs[i]->id==49475
        //     ||vs[i]->id==49478||vs[i]->id==72202
        //     ||vs[i]->id==72511||vs[i]->id==49487||
        //     vs[i]->id==72204||vs[i]->id==72210)
        // {
        //     printf("vid :%d %lf %lf %lf dis:%lf \n dis1:%lf mark:%d vid:%d %lf %lf %lf\n",vs[i]->id,
        //         vs[i]->point[0],vs[i]->point[1],vs[i]->point[2],
        //         dis,
        //         dis1,
        //         marks[i],v->id,
        //         v->point[0],v->point[1],v->point[2]);

        // }
    }

}

Mesh* get_mesh_from_rbtree_cells(Int_RB_Tree* cells)
{
    // printf()
    Mesh* re=(Mesh*)malloc(sizeof(Mesh));
    Mesh_init(re);
    re->dimension=2;re->simplex=1;re->manifold_require=1;
    Int_RB_Tree* treev = (Int_RB_Tree*)malloc(sizeof(Int_RB_Tree));
    int_rb_tree_init(treev);
    // Node* nodev = NULL;
    // int i = 0;

    for (auto it = cells->begin(cells); it.it != NULL; it++)
    {
        template_c* c = (template_c*)(it.second);
        // printf("cid:%d\n",c->id);
        template_v*vs[3]={0};
        for (int j = 0; j < 3; j++)
        {
            vs[j]= (template_v*)(treev->find(treev, c->vertices[j]->id));
            if ( vs[j]== NULL)
            {
                // int* value = (int*)malloc(sizeof(int));
                // *value = i;

                template_v* nv=re->create_vertexv(re,c->vertices[j]->point,3);
                treev->insert(treev, c->vertices[j]->id, nv);
                vs[j]=nv;
                // nodev = node_overlying(nodev, c->vertices[j]);
                // i++;
            }
        }
        re->create_cellv(re,vs,3);

    }


    // int_rb_tree_free_value(treev);
    int_rb_tree_free(treev);
    return re;
}

static Node* my_get_impact_scope_vertices2(
    Int_RB_Tree* treevs,int impact_scope)
{
    Int_RB_Tree* treev=(Int_RB_Tree*)malloc(sizeof(Int_RB_Tree));
    int_rb_tree_init(treev);
    LB_Vector_voidptr vec1,vec2;
    lb_vector_init_voidptr(&vec1);
    lb_vector_init_voidptr(&vec2);
    LB_Vector_voidptr* vecp1=&vec1,*vecp2=&vec2;
    // Node* tempn=NULL;
    // for(Node* nit=nodev;nit!=NULL;nit=(Node*)(nit->Next))
    for(auto it=treevs->begin(treevs);it.it!=NULL;it++)
    {
        template_v* v=(template_v*)(it.second);
        vecp1->push_back(vecp1,v);
        treev->insert(treev,v->id,v);
    }

    for(int num=0;num<impact_scope;num++)
    {
        vecp2->clear(vecp2);
        for(int i=0;i<vecp1->size;i++)
        {
            template_v*v=(template_v*)(*(vecp1->at(vecp1,i)));
            LB_Vector_voidptr vec= Mesh_vv_beginn(NULL,*v);
            for(int j=0;j<vec.size;j++)
            {
                template_v*vv=(template_v*)(*(vec.at(&vec,j)));
                if(treev->find(treev,vv->id)==NULL)
                {
                    treev->insert(treev,vv->id,vv);
                    vecp2->push_back(vecp2,vv);
                }
            }
            vec.clear(&vec);
        }
        vecp1->clear(vecp1);
        LB_Vector_voidptr* tempp=vecp1;
        vecp1=vecp2;
        vecp2=tempp;
    }

    vec1.clear(&vec1);
    vec2.clear(&vec2);
    Node* re=NULL;
    for(auto it=treev->begin(treev);it.it!=NULL;it++)
    {
        re=node_overlying(re, it.second);
    }
    int_rb_tree_free(treev);
    return re;
}

static int get_nearest_zhongzhixian_vertices(KD_Node* kdtreez,
    Mesh* lianjie)
{
    int re=0;
    for(auto vit=lianjie->v_begin(lianjie);vit!=lianjie->v_end(lianjie);vit++)
    {
        // KD_Node * kdnode=kd_tree_find_nearestp(lianjie.v[i],3, kdtreez);
        KD_Node*kdnode= kd_tree_find_nearest(quote(vit),kdtreez);

        if(distance_of_two_points(kdnode->value->point,quote(vit)->point, 3)<0.6)
        {re++;}

    }

    return re;
}
void test_lawen()
{
    clock_t start_time = clock();

    Mesh zhongzhi1;
    Mesh_init(&zhongzhi1);

    _ReadOff_(&zhongzhi1,"5(3)/zzx/zhongzhixian_.off",3);
    KD_Node*kdtree3= create_kd_tree_from_mesh(&zhongzhi1);

    Mesh zhijia;
    Mesh_init(&zhijia);
    _ReadOff_(&zhijia,"5(3)/ActiveBracket.off",3);
    KD_Node*kdtree= create_kd_tree_from_mesh(&zhijia);
// guweiwang_RetentionNet
    Mesh guwei1;
    Mesh_init(&guwei1);

    _ReadOff_(&guwei1,"5(3)/gww/guweiwang_RetentionNet.off",3);
    Node* vertices=NULL;
    for(auto vit=guwei1.v_begin(&guwei1);vit!=guwei1.v_end(&guwei1);vit++ )
    {
        vertices=node_overlying(vertices,quote(vit));
    }
    Node* vertices1=NULL;
    for(auto vit=zhongzhi1.v_begin(&zhongzhi1);
         vit!=zhongzhi1.v_end(&zhongzhi1);vit++ )
    {
        vertices=node_overlying(vertices,quote(vit));
        vertices1=node_overlying(vertices1,quote(vit));
    }
    Node* nodev1=downsampling_through_size(vertices1,0.5 );

    KD_Node*kdtreez= create_kd_tree_from_node(nodev1);

    Node*nodev= downsampling_through_size(vertices,0.5 );
    printf("nodev size:%d\n",node_size(nodev));

    KD_Node*kdtree2= create_kd_tree_from_node(nodev);
    // KD_Node*kdtree2= create_kd_tree_from_mesh(&guwei1);

    // KD_Node* kdtree= create_kd

    Mesh dalian1;
    Mesh_init(&dalian1);
    _ReadOff_(&dalian1,"5(3)/dljt/dalianjieti1_piece.off",3);

    Mesh dalian2;
    Mesh_init(&dalian2);
    _ReadOff_(&dalian2,"5(3)/dljt/dalianjieti2_piece.off",3);

    Mesh dalian3;
    Mesh_init(&dalian3);
    _ReadOff_(&dalian3,"5(3)/dljt/dalianjieti3_piece.off",3);

    Mesh* dalian=&dalian1;
    if(dalian2.num_v(&dalian2)>dalian->num_v(dalian))
    {
        dalian=&dalian2;
    }
    if(dalian3.num_v(&dalian3)>dalian->num_v(dalian))
    {
        printf("dalian3**********\n");
        dalian=&dalian3;
    }

    KD_Node* kdtree1=create_kd_tree_from_mesh(dalian);
    dalian->external_cells_init_(dalian);

    int max_size=0;template_c* max_cell=NULL;

    for(Node* nit=dalian->external_cells;nit!=NULL;nit=(Node*)(nit->Next))
    {
        template_c*c=(template_c*)(nit->value);
        if(c->vertices_size>max_size)
        {
            max_cell=c;
            max_size=c->vertices_size;
        }
    }
    KD_Node* kdtreeb= create_kd_tree(max_cell->vertices,max_cell->vertices_size,0);


    int len=zhijia.num_v(&zhijia);
    template_v**vs=(template_v**)malloc(sizeof(template_v*)*len);
    int* marks=(int*)malloc(sizeof(int)*len);
    memset(marks, 0, sizeof(int)*len);

    // memset(vs, 0, );
    int i=0;
    for(auto vit=zhijia.v_begin(&zhijia);
        vit!=zhijia.v_end(&zhijia);i++,vit++)
    {
        // if(quote(vit)->id==49508)
        // {
        //     printf("id:%d point:%lf %lf %lf\n",quote(vit)->id,
        //         quote(vit)->point[0],quote(vit)->point[1],quote(vit)->point[2]);
        //     LB_Vector_voidptr vec= Mesh_vv_beginn(NULL, *vit);
        //     for(int i=0;i<vec.size;i++)
        //     {
        //         printf("%d \n",((template_v* )(*vec.at(&vec,i)))->id );
        //         // 49475
        //         // 49478
        //         // 72202
        //         // 72511
        //         // 49487
        //         // 72204
        //         // 72210
        //     }

        //     vec.clear(&vec);
        // }
        vs[i]=quote(vit);
        // KD_Node*kdnode=kd_tree_find_nearest(quote(vit),kdtree1);
        // template_v* v=kdnode->value;
        // if(distance_of_two_points(v->point,quote(vit)->point , 3)<2.5)
        // {
        // }

    }

    mark_vertices(vs,marks,len,kdtree1,
        kdtree2,kdtree3,kdtreeb);



    Int_RB_Tree* cells=(Int_RB_Tree*)malloc(sizeof(Int_RB_Tree));
    int_rb_tree_init(cells);
    for(i=0;i<len;i++)
    {
        if(marks[i]==1)
        {
            for(int j=0;j<vs[i]->cellsn.size;j++)
            {
                template_c* c=(template_c*)(*(lb_vector1_at_voidptr(&(vs[i]->cellsn),j)) );
                // cells->insert(cells,vs[i]->cellsn.at( ,j ) );
                cells->insert(cells,c->id,c);
            }
        }
    }

    Node*sps=  split_unicom_region( cells);
    Int_RB_Tree* max_cells=NULL;
    for(Node* nit=sps;nit!=NULL;nit=(Node*)(nit->Next))
    {
        Int_RB_Tree* tempc=(Int_RB_Tree*)(nit->value);
        if(max_cells==NULL||tempc->size>max_cells->size)
        {
            max_cells=tempc;
        }
    }

    free_node(sps);

    Mesh* m1=get_mesh_from_rbtree_cells(max_cells);

    m1->external_cells_init_(m1);
    max_size=0;max_cell=NULL;
    for(Node* nit=m1->external_cells;nit!=NULL;nit=(Node*)(nit->Next))
    {
        template_c*c=(template_c*)(nit->value);
        if(c->vertices_size>max_size)
        {
            max_cell=c;
            max_size=c->vertices_size;
        }
    }

    Int_RB_Tree* treevs=(Int_RB_Tree*)malloc(sizeof(Int_RB_Tree));
    int_rb_tree_init(treevs);
    for(int i=0;i<max_cell->vertices_size;i++)
    {
        treevs->insert(treevs,max_cell->vertices[i]->id,max_cell->vertices[i]);
    }
    Node*delete_nodev= my_get_impact_scope_vertices2(treevs,4);
    // for(Node* nit=delete_nodev;nit!=NULL;nit=(Node*)(nit->Next))
    // {
    //     template_v* v=(template_v*)(nit->value);
    //     m1->delete_vertex(m1,*v,true);

    // }

    Node*sps1=  split_unicom_region( m1->cells);

    max_cells=NULL;
    for(Node* nit=sps1;nit!=NULL;nit=(Node*)(nit->Next))
    {
        Int_RB_Tree* tempc=(Int_RB_Tree*)(nit->value);
        if(max_cells==NULL||tempc->size>max_cells->size)
        {
            max_cells=tempc;
        }
    }

    free_node(sps1);
    Mesh* m2=get_mesh_from_rbtree_cells(max_cells);

    _WriteOff_(m2,"out.off");
    // for(auto vit=dalian->v_begin(dalian);
    //     vit!=dalian->v_end(dalian);vit++)
    // {
    // }
    // Mesh_init(&dalian);

    int_rb_tree_free(cells);

    clock_t end_time = clock();
    double elapsed_time = (double)(end_time - start_time) / CLOCKS_PER_SEC;
    printf("test_lawen function runtime: %.5f seconds\n", elapsed_time);
}
int main()
{
    test_lawen();
    // test();

    printf("endhere\n");
    return 0;
}
